/**
 * @file attitude_estimator.c
 * @brief Attitude Estimator implementation (Complementary Filter)
 * @author Flight Control Team
 * @date 2025-10-31
 */

#include "attitude_estimator.h"
#include <math.h>
#include <string.h>
#include <stdbool.h>

/* Complementary filter coefficient */
#define ALPHA 0.98f

/* Attitude state */
static Attitude_t attitude = {0};
static bool initialized = false;

/**
 * @brief Initialize attitude estimator
 */
int AttitudeEstimator_Init(void)
{
    memset(&attitude, 0, sizeof(Attitude_t));
    initialized = true;
    return 0;
}

/**
 * @brief Update attitude estimate using complementary filter
 */
int AttitudeEstimator_Update(float accel_x, float accel_y, float accel_z,
                             float gyro_x, float gyro_y, float gyro_z,
                             float mag_x, float mag_y, float mag_z,
                             float dt)
{
    if (!initialized) {
        return -1;
    }
    
    /* Calculate roll and pitch from accelerometer */
    float accel_roll = atan2f(accel_y, sqrtf(accel_x * accel_x + accel_z * accel_z));
    float accel_pitch = atan2f(-accel_x, sqrtf(accel_y * accel_y + accel_z * accel_z));
    
    /* Integrate gyroscope */
    float gyro_roll = attitude.roll + gyro_x * dt;
    float gyro_pitch = attitude.pitch + gyro_y * dt;
    float gyro_yaw = attitude.yaw + gyro_z * dt;
    
    /* Complementary filter */
    attitude.roll = ALPHA * gyro_roll + (1.0f - ALPHA) * accel_roll;
    attitude.pitch = ALPHA * gyro_pitch + (1.0f - ALPHA) * accel_pitch;
    
    /* Calculate yaw from magnetometer */
    if (mag_x != 0.0f || mag_y != 0.0f) {
        float mag_x_comp = mag_x * cosf(attitude.pitch) + mag_z * sinf(attitude.pitch);
        float mag_y_comp = mag_x * sinf(attitude.roll) * sinf(attitude.pitch) + 
                          mag_y * cosf(attitude.roll) - 
                          mag_z * sinf(attitude.roll) * cosf(attitude.pitch);
        
        float mag_yaw = atan2f(-mag_y_comp, mag_x_comp);
        attitude.yaw = ALPHA * gyro_yaw + (1.0f - ALPHA) * mag_yaw;
    } else {
        attitude.yaw = gyro_yaw;
    }
    
    /* Store angular rates */
    attitude.roll_rate = gyro_x;
    attitude.pitch_rate = gyro_y;
    attitude.yaw_rate = gyro_z;
    
    return 0;
}

/**
 * @brief Get current attitude
 */
int AttitudeEstimator_GetAttitude(Attitude_t *att)
{
    if (!initialized || att == NULL) {
        return -1;
    }
    
    memcpy(att, &attitude, sizeof(Attitude_t));
    return 0;
}

